#include <iostream>

#include "gtest/gtest.h"

#include "arena_pole_cart.h"

using namespace std;

TEST(Arena, Pole_cart_test)
{
  Arena_pole_cart *arena = new Arena_pole_cart();

  // start with small positive angle of pole
  ASSERT_TRUE( *(arena->get_theta_p())> 0.0);

  arena->update(1.0);


  arena->push(1.0);

  arena->update(2.0);

  ASSERT_EQ(2.0, arena->get_time());

  ASSERT_TRUE( *(arena->get_cart_position_p())> 0.0);
  ASSERT_TRUE( *(arena->get_theta_p())> 0.0);

  arena->reset();

  ASSERT_EQ( 0.0, *(arena->get_cart_position_p()) );

  // start with small positive angle of pole
  ASSERT_TRUE( *(arena->get_theta_p())> 0.0);

  arena->push(-1.0);

  arena->update(1.0);

  ASSERT_TRUE( *(arena->get_cart_position_p())< 0.0);
  ASSERT_TRUE( *(arena->get_theta_p())> 0.0);
}
